#include "path_plan.h"

pathPlan_NS::State::State(double x, double y, double phi, double v) {
    this->x_ = x;
    this->y_ = y;
    this->phi_ = phi;
    this->v_ = v;
    this->state_.resize(6);
}

void pathPlan_NS::State::calError(const VectorXd &coeffs) {
    this->cte_ = polyFit_NS::polyEval(coeffs, x_) - y_;     //路径的y坐标减去车辆的y坐标
    this->ephi_ = phi_ - atan(coeffs[1] + 2 * coeffs[2] * x_ + 3 * coeffs[3] * x_ * x_);
    this->state_ << x_, y_, phi_, v_, cte_, ephi_;

}

pathPlan_NS::DesiredPath::DesiredPath() {
    waypoints_x_ = myNumpy::readTxt<double>("/home/hyh/coder/cpp/ros_simulink/src/path/mpc/x.txt");
    waypoints_y_ = myNumpy::readTxt<double>("/home/hyh/coder/cpp/ros_simulink/src/path/mpc/y.txt");
    Eigen::Map<VectorXd> temp_x(waypoints_x_.data(), waypoints_x_.size());
    Eigen::Map<VectorXd> temp_y(waypoints_y_.data(), waypoints_y_.size());
    this->coeffs_ = polyFit_NS::polyFit(temp_x, temp_y, 3);    //用三阶多项式拟合路径，并且生成相应的系数
}

